/******************************************************************
基于串口通信的ROS小车基础控制器，功能如下：
1.实现ros控制数据通过固定的格式和串口通信，从而达到控制小车的移动
2.订阅了/cmd_vel主题，只要向该主题发布消息，就能实现对控制小车的移动


*******************************************************************/
#include "ros/ros.h"  //ros需要的头文件
#include <geometry_msgs/Twist.h>
#include <rosSerial/rosSerialMsg.h>
#include <rosSerial/ptz.h>
#include <string>        
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include <serial/serial.h>                //ROS已经内置了的串口包 
#include <signal.h>
#include <pthread.h>
#include <ros/time.h>

using  namespace std;
/****************************************************************************/
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;
/*************************参数定义****************************************/
#define  ENcoder  1024
#define  WHEELDIA 398
#define  DIAMETER 200
#define  RATIO    20
#define  MAXCNT   200
/*****************************************************************************/
unsigned char Rxbuff[MAXCNT];
unsigned char Txbuff[MAXCNT];
float ratio = 1000.0f ;   //转速转换比例，执行速度调整比例
float D = 0.359000f ;    //中心点到轮子的距离
float linear_temp=0,angular_temp=0;//暂存的线速度和角速度

string rec_buffer,rxbuff;  //串口数据接收变量

//发送给下位机的左右轮速度，里程计的坐标和方向
union ShorttData //union的作用为实现char数组和float之间的转换
{
     short int d;
     char data[2];
}left_speed_data,right_speed_data,F_right_speed_data,F_left_speed_data,B_right_speed_data,B_left_speed_data,FrameHead,FrameTail,FrameCMD,Framechecksum,MFramechecksum,MFrameCMD,MElectric,MState,MPower;
union simleData
{
    char  data[1];
    char d;
}FrameType,DATALEN,FrameLen,MDATALEN,MFramelength;
void InitRAWData();
void SendData();
void ReadData();
void WriteInitDataVel();
void WriteSendData();
void PtzWriteInitDataVel();
void PtzWriteSendData(short int Turn,short int Pitch);
void OFFReadData(void);

serial::Serial my_serial;
ros::Publisher  pub;
/************************************************************/
void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
{
   float RightVel,LeftVel; 
   //string port("/dev/ttyUSB0");    //小车串口号
    //unsigned long baud = 115200;    //小车串口波特率
    //my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口

    angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s
    linear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s

    //将转换好的小车速度分量为左右轮速度
    LeftVel  = linear_temp - 0.5f*angular_temp*D ;
    RightVel = linear_temp + 0.5f*angular_temp*D ;
    //存入数据到要发布的左右轮速度消息
    left_speed_data.d=LeftVel*1000;   //放大１０００倍，mm/s
    right_speed_data.d=RightVel*1000;//放大１０００倍，mm/s
   
   //  cout<<left_speed_data.d<<"  "<<right_speed_data.d<<endl;  
   
    WriteInitDataVel();
    WriteSendData();

    //写入数据到串口
    my_serial.write(Rxbuff,23);
}



void Ptzcallback(const rosSerial::ptz::ConstPtr &msg)
{
    ROS_INFO("Send PTZ CMD\n");
    PtzWriteInitDataVel();
    PtzWriteSendData(msg->PtzTurn,msg->PtzPitch);
    //写入数据到串口
    my_serial.write(Rxbuff,23);
}



/*******************************************接收线程**************************************************/
void *receive_func(void* param)  
{
    int len =0,FirstFlag=0;
    rosSerial::rosSerialMsg VelSPeed;
    ros::Time current_time, now_time;

    ROS_INFO("run thread\n");  
    while(ros::ok())
    {
        now_time = ros::Time::now();

        rec_buffer.clear();
        my_serial.read(rec_buffer,25);
        //rec_buffer =my_serial.readline(25,"\n");    //获取串口发送来的数据
        const char *receive_data=rec_buffer.data(); //保存串口发送来的数据
        if(rec_buffer.length()==25) //串口接收的数据长度正确就处理并发布里程计数据消息
        {  
            if(rec_buffer[23] == 0X0D && rec_buffer[24] == 0X0A)
            {    
                current_time = ros::Time::now();
                if(rec_buffer[2] == 0x19)
                {       
                     for(int k=0;k<2;k++)//
                    {

                        F_right_speed_data.data[k]=receive_data[k+7];
                        F_left_speed_data.data[k]=receive_data[k+9];
                        B_right_speed_data.data[k]=receive_data[k+11];
                        B_left_speed_data.data[k]=receive_data[k+13];
                        MPower.data[k]= receive_data[k+15];
                        MElectric.data[k]= receive_data[k+17];
                        MState.data[k]   = receive_data[k+19];
                        
                    }
                    
                    VelSPeed.FLVel =  (float)F_left_speed_data.d/1000.0;
                    VelSPeed.FRVel =  (float)F_right_speed_data.d/1000.0;
                    VelSPeed.BLVel =  (float)B_left_speed_data.d/1000.0;
                    VelSPeed.BRVel =  (float)B_right_speed_data.d/1000.0;
                    VelSPeed.Electric= MElectric.d;
                    VelSPeed.state    = MState.d;
                    VelSPeed.Power    = MPower.d;
                    //printf("FL:%.2f FR:%.2f BL:%.2f BR:%.2f ST:%d PO:%d\n",VelSPeed.FLVel,VelSPeed.FRVel,VelSPeed.BLVel,VelSPeed.BRVel,VelSPeed.state,VelSPeed.Power);
                    pub.publish(VelSPeed);   
                    my_serial.read(rec_buffer,175);   
                }
            }
        }     
        else
        {
            
            if(FirstFlag == 0)
            {
                FirstFlag = 1;
                current_time = ros::Time::now();
            }
            if(FirstFlag == 1 && (now_time - current_time).toSec() >= 1)
            {
                FirstFlag = 0;
                //清空数据残余
                len = my_serial.available();
                 if(len > 0)
                 {
                    my_serial.read(rec_buffer,len);
                 }
                ReadData();
                my_serial.write(Txbuff,11);
                ROS_INFO("ON 20ms data up\n");   
            }
        }
        
    }
    ROS_INFO("run thread exit\n");//退出接收线程    
    pthread_exit(0);
}

//当关闭包时调用，关闭20ms上传
void mySigIntHandler(int sig)
{
   OFFReadData();
   ROS_INFO("close the serial!\n");
   //my_serial.close();
   ros::shutdown();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "base_controller",ros::init_options::NoSigintHandler);//初始化串口节点
    signal(SIGINT, mySigIntHandler);                                             //把信号槽连接到mySigIntHandler保证关闭节点时能够关闭20ms数据上传
    ros::NodeHandle n;  //定义节点进程句柄
   

    try 
    { 
         //设置串口属性，并打开串口 
        my_serial.setPort("/dev/ttyUSB0"); 
        my_serial.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(2000); 
        my_serial.setTimeout(to); 
        my_serial.open(); 
    }
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 

    //检测串口是否已经打开，并给出提示信息 
    if(my_serial.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    } 
   
    ReadData(); 
    my_serial.write(Txbuff,11);
        
    ros::Subscriber sub = n.subscribe("/SerialPort/cmd_vel", 100, callback); //订阅/cmd_vel主题
    ros::Subscriber sub2= n.subscribe("/turtlebot_teleop/cmd_vel", 20, callback); //订阅/cmd_vel主题
   // ros::Subscriber sub1= n.subscribe("cmd_ptz", 20, Ptzcallback); //订阅/cmd_ptz主题

    pub = n.advertise<rosSerial::rosSerialMsg>("VelSpeed",20);

    int m_run0=1;
    pthread_t threadid;
    int ret=pthread_create(&threadid,NULL,receive_func,&m_run0);

    
    ros::Rate loop_rate(20);//设置周期休眠时间
    while(ros::ok())
    {        
        ros::spinOnce();//周期执行
        loop_rate.sleep();//周期休眠
    }
    return 0;
    /*
    ros::spin();
    return 0; 
    */
}
/*******************************************************************
Fun:InitRAWData
Descript:填充底盘参数数据
input：NONE
return：NONE
*********************************************************************/
void InitRAWData()
{
     FrameHead.d = 0XDEED;
     FrameLen.d  =0X0B;
     FrameType.d =0X04;
     FrameCMD.d  =0X8007;
     DATALEN.d   = 00;
     Framechecksum.d = 0X0261;
     FrameTail.d     = 0X0A0D;
}

void SendData()
{

    Txbuff[0] = FrameHead.data[0];
    Txbuff[1] = FrameHead.data[1];
    Txbuff[2] = FrameLen.data[0];
    Txbuff[3] = FrameType.data[0];
    Txbuff[4] = FrameCMD.data[0];
    Txbuff[5] = FrameCMD.data[1];
    Txbuff[6] = DATALEN.data[0];
    Txbuff[7] = Framechecksum.data[0];
    Txbuff[8] = Framechecksum.data[1];
    Txbuff[9] = FrameTail.data[0];
    Txbuff[10] =FrameTail.data[1];
    // for(int i=0;i<11;i++)
    // {
    //     printf("[%x]",Txbuff[i]);
    // }

}
void ReadData()
{
	InitRAWData();
	SendData();

}

void OFFReadData(void)
{
    InitRAWData();
    SendData();
    Txbuff[7] = Framechecksum.data[0]+1;
    my_serial.write(Txbuff,11);
}

void WriteInitDataVel()
{
     FrameHead.d     = 0XDEED;
     MFramelength.d  =0X17;
     FrameType.d =0X04;
     FrameCMD.d = 0X8002;
     MDATALEN.d   = 0X06;
     FrameTail.d = 0X0A0D;
}

void PtzWriteInitDataVel()
{
     FrameHead.d     = 0XDEED;
     MFramelength.d  =0X17;
     FrameType.d =0X04;
     FrameCMD.d = 0X8003;
     MDATALEN.d   = 0X06;
     FrameTail.d = 0X0A0D;
}


void WriteSendData()
{

     
    MFramechecksum.d = 0;

    Rxbuff[0] = FrameHead.data[0];
    Rxbuff[1] = FrameHead.data[1];
    Rxbuff[2] = MFramelength.data[0];
    Rxbuff[3] = FrameType.data[0];
    Rxbuff[4] = FrameCMD.data[0];
    Rxbuff[5] = FrameCMD.data[1];
    Rxbuff[6] = MDATALEN.data[0];
   
  
     Rxbuff[7]  = right_speed_data.data[0];
     Rxbuff[8]  = right_speed_data.data[1]; 

     Rxbuff[9]  = left_speed_data.data[0];
     Rxbuff[10] = left_speed_data.data[1];

     Rxbuff[11]  = right_speed_data.data[0];
     Rxbuff[12]  = right_speed_data.data[1];

     Rxbuff[13]   =left_speed_data.data[0];
     Rxbuff[14]  = left_speed_data.data[1];

     Rxbuff[15]  = 0;
     Rxbuff[16]  = 0;

     Rxbuff[17]   =0;
     Rxbuff[18]  = 0;

   

     
  int j;
  int i;
  short int  Tempdata=0;
  for(j=0;j<MFramelength.d-4;j++)
  {
   Tempdata= Tempdata+Rxbuff[j];

  }
    MFramechecksum.d = Tempdata;
    for(i=0;i<2;i++)
    {
     Rxbuff[i+19]  = MFramechecksum.data[i];
     Rxbuff[i+21]   = FrameTail.data[i];

    }
}

void PtzWriteSendData(short int Turn,short int Pitch)
{

     
    MFramechecksum.d = 0;

    Rxbuff[0] = FrameHead.data[0];
    Rxbuff[1] = FrameHead.data[1];
    Rxbuff[2] = MFramelength.data[0];
    Rxbuff[3] = FrameType.data[0];
    Rxbuff[4] = FrameCMD.data[0];
    Rxbuff[5] = FrameCMD.data[1];
    Rxbuff[6] = MDATALEN.data[0];
   
  
     Rxbuff[7]  = Turn & 0x00FF;
     Rxbuff[8]  = (Turn >> 8) & 0x00FF; 

     Rxbuff[9]  = Pitch & 0x00FF;
     Rxbuff[10] = (Pitch >> 8) & 0x00FF; 

     Rxbuff[11]  = 0;
     Rxbuff[12]  = 0;

     Rxbuff[13]  = 0;
     Rxbuff[14]  = 0;

     Rxbuff[15]  = 0;
     Rxbuff[16]  = 0;

     Rxbuff[17]   =0;
     Rxbuff[18]  = 0;

   

     
  int j;
  int i;
  short int  Tempdata=0;
  for(j=0;j<MFramelength.d-4;j++)
  {
   Tempdata= Tempdata+Rxbuff[j];

  }
    MFramechecksum.d = Tempdata;
    for(i=0;i<2;i++)
    {
     Rxbuff[i+19]  = MFramechecksum.data[i];
     Rxbuff[i+21]   = FrameTail.data[i];

    }
}
